/** Interactive Surface  FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *
 *  @author Acer
 */
package coordinates;

/** Interactive Surface FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *  General Information about this class -
 *  Comments -
 */
public class Kalman {

    double kalman_q, kalman_p, kalman_r, kalman_x, kalman_k;

    void initKalman(double q1, double r1, double p1, double initValue) {
        kalman_q = q1;
        kalman_r = r1;
        kalman_p = p1;
        kalman_x = initValue;
    }

    void kalmanUpdate(double measurement) {
        //prediction update
        //omit x = x
        kalman_p = kalman_p + kalman_q;
        //measurement update
        kalman_k = kalman_p / (kalman_p + kalman_r);
        kalman_x = kalman_x + kalman_k * (measurement - kalman_x);
        kalman_p = (1 - kalman_k) * kalman_p;
    }

    public double kalmanPredict() {
       // System.out.println(kalman_x);
        return kalman_x;
    }
}
